/*************************************************************************/
/*     PB6 is connected to the base - SLEW - Servo                       */
/*     Equation of motion is y = 0.5442x^2 + 345.16x + 47728             */
/*     where y is the match value, x is the angle (degrees)              */
/*     positive x is CCW from above                                      */
/*                                                                       */
/*     PB7 is connected to the secondary - LUFF - servo                  */
/*     y = 0.0632x^2 - 355.56x + 50688                                   */
/*     positive x is luff upwards                                        */
/*                                                                       */
/*     Units are degrees                                                 */
/*                                                                       */
/*************************************************************************/
/*     To determine the angle, use the transform                         */
/*     angle = arctan(coord/range)                                       */
/*     where coord is the coordinate you want (in metres)                */
/*     and range is the range to the target, at zero degrees.            */
/*     Range will be determined from the ultrasonic rangefinder          */
/*     module.                                                           */
/*                                                                       */
/*                                                                       */
/*                                                                       */
/*                                                                       */
/*                                                                       */
/*                                                                       */
/*************************************************************************/
/* Command Strings                                                       */
/*                                                                       */
/*     $LTCON,xVal,yVal*chk                                              */
/*       LTCON - Laser Turret Control                                    */
/*       xVal  - X Coordinate, as a signed float                         */
/*       yVal  - Y Coordinate, as a signed float                         */
/*       chk   - Checksum, calculated as below                           */
/*                                                                       */
/*     $LTANG,pan,tilt*chk                                               */
/*       LTANG - Laser Turret Angle                                      */
/*       pan   - pan angle, as a signed float                            */
/*       tilt  - tilt angle, as a signed float                           */
/*       chk   - Checksum, calculated as below                           */
/*                                                                       */
/*     $LTRAN,0*chk                                                      */
/*       LTRAN - Laser Turret Range                                      */
/*       0     - Null, Required for parser                               */
/*       chk   - Checksum, calculated as below                           */
/*                                                                       */
/*     $LTHRT,0*chk                                                      */
/*       LTHRT - Laser Turret Heartbeat                                  */
/*       0     - Null, Required for parser                               */
/*       chk   - Checksum, calculated as below                           */
/*                                                                       */
/*  int checksum = 0;                                                    */
/*  for (inti = 0; i < stringToCalculateTheChecksumOver.length; i++){    */
/*  checksum ^= Convert.ToByte(sentence[i]);}                            */
/*                                                                       */
/*                                                                       */
/*                                                                       */
/*************************************************************************/

#include "inc/hw_types.h"
#include "inc/lm4f120h5qr.h"
#include "driverlib/mpu.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "inc/hw_memmap.h"
#include "Timers.h"
#include "InterruptRoutines.h"
#include "UART.h"
#include "Servos.h"
#include "RangeFinder.h"
#include "Parse_String.h"
#include <stdio.h>
#include <string.h>
#include <math.h>

#define FCPU 40000000 //40MHz

volatile unsigned int RxBufferIndex;
char RxBuffer[200];

volatile unsigned int frameStart;

volatile unsigned long count, rangeStart, rangeEnd;
volatile unsigned int request=0, rangeValid=0, stringAvailable = 0;
volatile float panAngle, tiltAngle, zeroRange;

volatile float x_VAL, y_VAL;


volatile unsigned int debug=0;


char outString[50];



/* Main */
void main(){


	volatile unsigned long ulLoop;
	volatile int index = 0;
	volatile float xVal = 1, yVal = 0.5, xSign, ySign;
	volatile unsigned int stringLength;

	volatile int parseStringStatus = 0;

	//Setup the system clock - 40MHz
	SysCtlClockSet(SYSCTL_SYSDIV_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);


	UART4_Buffer_Init();
	UART4_Init();
	Timer2_Init();
	Timer1_Init();
	Timer0_Init();
	Interrupt_Enable();

	zeroRange = findRangeAtZero();


	while(1){




/*		if(request){
			zeroRange = findRangeAtZero();
			request = 0;
		}*/


		if(stringAvailable){
			parseStringStatus = parseString(&RxBuffer[0]);
			stringAvailable = 0;
		}


	}  //main while
}  //main

